#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "robot_driver.hpp"

/**
 * @brief return x if abs(x) < limit, else return limit (x > 0) or -limit (x < 0)
 *
 * @tparam Ta number comparable type
 * @tparam Tb number comparable type
 * @param x value
 * @param limit limit
 * @return Ta
 */
template <typename Ta, typename Tb> Ta limit_abs(Ta x, Tb limit)
{
	if (limit < 0) return 0;
	return x > limit ? limit : (x < -limit ? -limit : x);
}

class odom_node : public rclcpp::Node
{
  private:
	// receive cmd vel
	void cmdvel_callback(geometry_msgs::msg::Twist msg)
	{
		// message assert
		if (msg.angular.x != 0 || msg.angular.y != 0)
		{
			RCLCPP_INFO(this->get_logger(),
						"twist.angular x or y non-zero, only z will be accepted");
		}
		if (msg.linear.z != 0 || msg.linear.y != 0)
		{
			RCLCPP_INFO(this->get_logger(),
						"twist.linear z or y non-zero, only x will be accepted");
		}
		if (abs(msg.linear.x) > 5)
		{
			RCLCPP_INFO(this->get_logger(), "twist.linear x exceed limit, set to 5");
			msg.linear.x = limit_abs(msg.linear.x, 5);
		}

		// update cmd
		if (this->cmdvel_mtx.try_lock_for(std::chrono::milliseconds(10)))
		{
			// this value will be copied by cmdvel thread later
			cmdvel_cur = msg;
			cmdvel_updated = true;
			this->cmdvel_mtx.unlock();
		}
		else
			RCLCPP_WARN(this->get_logger(), "cmd vel ignored due to mutex time out");
	}

	// send driver data
	void cmdvel_thread_func()
	{
		static geometry_msgs::msg::Twist cmdvel;
		static double update_time = 0;

		while (rclcpp::ok())
		{
			// send command
			if (this->cmdvel_mtx.try_lock_for(std::chrono::milliseconds(10)))
			{
				if (this->cmdvel_updated)
				{
					// cmd updated, use new value
					cmdvel = this->cmdvel_cur;
					update_time = rclcpp::Clock().now().seconds();
					this->cmdvel_updated = false;
				}
				else
				{
					// not updated, keep old value for a period of time
					if (rclcpp::Clock().now().seconds() - update_time > 0.6)
					{
						// timeout, set zero
						RCLCPP_DEBUG(this->get_logger(), "cmdvel thread: no input for now");
						cmdvel.linear.x = 0;
						cmdvel.linear.y = 0;
						cmdvel.linear.z = 0;
						cmdvel.angular.x = 0;
						cmdvel.angular.y = 0;
						cmdvel.angular.z = 0;
					}
					else
					{
						// keep old value
					}
				}
				this->cmdvel_mtx.unlock();
			}
			else
			{
				// mutex lock failed
				RCLCPP_WARN(this->get_logger(), "cmdvel_thread unable to lock mutex");
			}

			// send driver data
			if (this->driver->driver_move(cmdvel.linear.x, cmdvel.angular.z) < 0)
			{
				RCLCPP_WARN(this->get_logger(), "cmdvel thread: error on driver move");
			}
			rclcpp::sleep_for(std::chrono::milliseconds(100));
		}
	}

	// topic subscriptions
	rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdvel_sub;
	std::shared_ptr<Driver> driver;
	// to sync with command thread
	std::timed_mutex cmdvel_mtx;
	geometry_msgs::msg::Twist cmdvel_cur; // protected by cmdvel_mtx
	bool cmdvel_updated; // protected by cmdvel_mtx, is set by cmdvel subscription callback,
						 // reset by cmdvel_func_thread
	// thread to actually send command to driver
	std::shared_ptr<std::thread> cmdvel_thread;

  public:
	odom_node(std::string name) : Node(name)
	{
		// declare parameters
		double wheel_base = 0.525, track_width = 0.4, wheel_diameter = 0.28;
		double scale_pos = 163800, scale_vel = 27.44;
		std::string remote_ip = "127.0.0.1";
		std::string local_ip = "127.0.0.1";
		bool loopback = false;
		std::string driver_model = "Omni";
		this->declare_parameter("wheel_base", wheel_base);
		this->declare_parameter("track_width", track_width);
		this->declare_parameter("wheel_diameter", wheel_diameter);
		this->declare_parameter("scale_pos", scale_pos);
		this->declare_parameter("scale_vel", scale_vel);
		this->declare_parameter("remote_ip", remote_ip);
		this->declare_parameter("local_ip", local_ip);
		this->declare_parameter("loopback", loopback);
		this->declare_parameter("driver_model", driver_model);

		this->get_parameter("wheel_base", wheel_base);
		this->get_parameter("track_width", track_width);
		this->get_parameter("wheel_diameter", wheel_diameter);
		this->get_parameter("scale_pos", scale_pos);
		this->get_parameter("scale_vel", scale_vel);
		this->get_parameter("remote_ip", remote_ip);
		this->get_parameter("local_ip", local_ip);
		this->get_parameter("loopback", loopback);
		this->get_parameter("driver_model", driver_model);

		if (driver_model == "Omnidirectional")
		{
			this->driver = std::make_shared<OmnidirectionalDriver>(
				wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				local_ip, loopback);
			RCLCPP_INFO(this->get_logger(), "driver model: Omnidirectional");
		}
		else if (driver_model == "Differential")
		{
			this->driver = std::make_shared<DifferentialDriver>(
				wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				local_ip, loopback);
			RCLCPP_INFO(this->get_logger(), "driver model: Differential");
		}
		else if (driver_model == "Ackerman")
		{
			this->driver = std::make_shared<AckermanDriver>(
				wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				local_ip, loopback);
			RCLCPP_INFO(this->get_logger(), "driver model: Ackerman");
		}
		else
		{
			this->driver = std::make_shared<OmnidirectionalDriver>(
				wheel_base, track_width, wheel_diameter, scale_pos, scale_vel, remote_ip,
				local_ip, loopback);
			RCLCPP_INFO(this->get_logger(),
						"unexpected driver model string, select Omnidirectional by default");
		}

		RCLCPP_INFO(this->get_logger(),
					"wheel_base: %lf, track_width: %lf, wheel_diameter: %lf, scale_pos: %lf, "
					"scale_vel: %lf, remote_ip: %s, local_ip: %s",
					wheel_base, track_width, wheel_diameter, scale_pos, scale_vel,
					remote_ip.c_str(), local_ip.c_str());
		// create robot speed command subscription
		this->cmdvel_sub = this->create_subscription<geometry_msgs::msg::Twist>(
			"cmd_vel", 1, std::bind(&odom_node::cmdvel_callback, this, std::placeholders::_1));
		// threads
		this->cmdvel_thread =
			std::make_shared<std::thread>(std::bind(&odom_node::cmdvel_thread_func, this));
		this->cmdvel_thread->detach();

		RCLCPP_INFO(this->get_logger(), "started cmdvel thread");
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<odom_node>("diff_driver_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
